//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <bsp.h>
#include <init.h>
#include <flash.h>

STATIC DECL_CODEINIT int FlashEraseSector(
        ulong_t ulFlashBase, long lSectorOffset);
STATIC DECL_CODEINIT void FlashWaitForProgram(ulong_t ulFlashBase);
STATIC DECL_CODEINIT int FlashWriteBuffer(
        ulong_t ulFlashBase, long lOffset, uchar_t *pucData, long lNumBytes);
STATIC DECL_CODEINIT int FlashReadBuffer(
        ulong_t ulFlashBase, long lOffset, uchar_t *pucData, long lNumBytes);

EXTERN_C DECL_CODEINIT void CDECL WriteFlash(BootInfo *pBootInfo)
{
    kprintf("BootInfo: WriteMagic = 0x%x, FlashSectorSize = 0x%x\n"
            "FlashOffset = 0x%x, ImageOffset = 0x%x, ImageSize = %d\n",
            pBootInfo->WriteMagic,
            pBootInfo->FlashSectorSize,
            pBootInfo->FlashOffset,
            pBootInfo->ImageOffset,
            pBootInfo->ImageSize);

    if (pBootInfo->WriteMagic != WRITE_MAGIC) return; //no need to write flash

    uint_t uFlashSectorSize = pBootInfo->FlashSectorSize;
    uint_t uFlashOffset     = pBootInfo->FlashOffset;
    uint_t uImageOffset     = pBootInfo->ImageOffset;
    uint_t uImageSize       = pBootInfo->ImageSize;

    kprintf("Write image (0x%x ~ 0x%x) to flash (0x%x ~ 0x%x) ...\n",
            uImageOffset, uImageOffset + uImageSize,
            uFlashOffset,
            RoundUp((uFlashOffset + uImageSize), uFlashSectorSize));

    for (uint_t uWriteSize =
            (uImageSize > uFlashSectorSize) ? uFlashSectorSize : uImageSize;
            uImageSize > 0; uImageSize -= uWriteSize) {
        uWriteSize =
            (uImageSize > uFlashSectorSize)? uFlashSectorSize : uImageSize;
        FlashEraseSector(FLASH_BASE, uFlashOffset);
        FlashWriteBuffer(
            FLASH_BASE, uFlashOffset, (uchar_t*)uImageOffset, uWriteSize);
        uImageOffset += uWriteSize;
        uFlashOffset += uFlashSectorSize;
    }

    kprintf("Write image done.\n");
}

//
// FlashEraseSector erases a specific sector of the flash ROM.
//
STATIC DECL_CODEINIT int FlashEraseSector(
                ulong_t ulFlashBase, long lSectorOffset)
{
    ushort_t * volatile pulPtr = (ushort_t *)ulFlashBase;

    // Make sure the specified sector is within the range of the flash ROM.
    if (lSectorOffset >= 0x01000000) {
        return -1;
    }

    // Write the erase command to the flash ROM.
    pulPtr[lSectorOffset >> 1] = 0x0020;
    pulPtr[lSectorOffset >> 1] = 0x00D0;

    // Wait until the sector erase has completed.
    FlashWaitForProgram(ulFlashBase);

    // Put the flash into read array mode.
    *pulPtr = 0x00FF;

    return 0;
}

//
// FlashWaitForProgram waits until bit 7 becomes one, indicating that the
// embedded program (be it a chip erase, sector erase, or data store program)
// has completed executing.
//
STATIC DECL_CODEINIT void FlashWaitForProgram(ulong_t ulFlashBase)
{
    ushort_t * volatile pulPtr = (ushort_t *)ulFlashBase;
    ushort_t volatile ulTemp;

    // kprintf("FlashWaitForProgram pulPtr=%08x\n", pulPtr);

    // Read from the flash ROM until bit 7 is one.
    while (1) {
        // Read the status register from the flash ROM.
        ulTemp = *pulPtr;

        // Mask off all bits except for bit 7.
        ulTemp &= 0x0080;

        // If bit 7 is one, then quit waiting.
        if (ulTemp == 0x0080) {
            break;
        }
    }
}

//
// FlashWriteBuffer writes  datas into the flash ROM by Write buffer command.
//
STATIC DECL_CODEINIT int FlashWriteBuffer(
                ulong_t ulFlashBase, long lOffset, uchar_t *pucData, long lNumBytes)
{
    ushort_t * volatile pulPtr = (ushort_t *)ulFlashBase;
    int volatile i;
    long lIdx;
    long lBitIdx;
    long lSavedOffset, lData32Mod = 0;
    uchar_t buf[32];

    lSavedOffset = lOffset;

    for (i = 0; i < 128 * 5; i++) {}    // Delay 128 us

    lData32Mod = lNumBytes & (32 - 1);
    lNumBytes = lNumBytes & ~(32 - 1);

    // Write the write to buffer command.
    for (lIdx = 0; lIdx < lNumBytes; lIdx += 32) {
        lOffset = ((lIdx + lSavedOffset) >> 17) << 17;

        // Write buffer data.
        pulPtr[lOffset >> 1] = 0xE8;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);

        // write countlegth and block address.
        pulPtr[lOffset >> 1] = 0xf;

        for (lBitIdx = 0; lBitIdx < 32; lBitIdx += 2) {
            pulPtr[(lBitIdx + lIdx + lSavedOffset) >> 1] =
                *((ushort_t *)(pucData + lBitIdx + lIdx));
        }

        // program buffer to flash confirm.
        pulPtr[lOffset >> 1] = 0xD0;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);
    }

    if (lData32Mod) {
        lOffset = lOffset + lNumBytes;
        lSavedOffset = lOffset;
        for (i = 0; i < 32; i++) {
            buf[i] = 0;
        }

        FlashReadBuffer(ulFlashBase, lOffset, buf, 32);
        for (i = 0; i < lData32Mod; i++) {
            buf[i] = *((uchar_t *)pucData + lNumBytes + i);
        }

        lOffset = (lSavedOffset>> 17) <<17;

        // Write buffer data.
        pulPtr[lOffset >> 1] = 0xE8;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);

        // write countlegth and block address.
        pulPtr[lOffset >> 1] = 0xf;

        for (lBitIdx = 0; lBitIdx < 32; lBitIdx += 2) {
            pulPtr[(lBitIdx + lSavedOffset) >> 1] =
                *((ushort_t *)(buf + lBitIdx));
        }

        // program buffer to flash confirm.
        pulPtr[lOffset >> 1] = 0xD0;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);
    }

    // Wait until the data word has been programmed.
    FlashWaitForProgram(ulFlashBase + lOffset);

    // Put the flash into read array mode.
    pulPtr[0] = 0x00FF;

    return 0;
}

// All above function come from flash.cpp of elame.dev
STATIC DECL_CODEINIT int FlashReadBuffer(
                ulong_t ulFlashBase, long lOffset,
                uchar_t *pucData, long lNumBytes)
{
    ushort_t * volatile pulPtr = (ushort_t *)ulFlashBase;
    long i;

    for (i = 0; i < lNumBytes; i += 2) {
        *((ushort_t *)(pucData + i)) = pulPtr[(lOffset + i) >> 1];
    }
    return 0;
}
